#pragma config(Sensor, in1,    pot_sensor,     sensorPotentiometer)
#pragma config(Sensor, in6,    right_line,     sensorLineFollower)
#pragma config(Sensor, in7,    middle_line,    sensorLineFollower)
#pragma config(Sensor, in8,    left_line,      sensorLineFollower)
#pragma config(Sensor, dgtl1,  front_right_limit_switch, sensorTouch)
#pragma config(Sensor, dgtl2,  back_right_limit_switch, sensorTouch)
#pragma config(Motor,  port1,           left_motor,    tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port6,           claw_motor,    tmotorVex393, openLoop)
#pragma config(Motor,  port7,           joint_motor,   tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port10,          right_motor,   tmotorVex393, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define balloffset 290

void arm_controller(int setpoint);
void forward_distance( float inches );
void backward_distance( float inches );
void turn_left_90();
void turn_right_90 ();
int scale = 1;
int setpoint = balloffset;


task main()
{
	bool robot_enable = false;
	bool line_follow = false;
	bool wall_follow = false;

	while(true)
	{

		arm_controller(setpoint);

		if(vexRT[Btn7U])
		{
			robot_enable=true;
			ClearTimer (T1);
		}

		if(robot_enable && time10[T1] < 12000)
		{


			motor[ left_motor ] = (vexRT[ Ch2]+vexRT[Ch4])/scale;
			motor[ right_motor ] = (vexRT[ Ch2]-vexRT[Ch4])/scale;

			if( vexRT[ Btn7D ] )
			{
				robot_enable = false;
				{
					motor[right_motor]=0;
					motor[left_motor]=0;
					motor[claw_motor]=0;
					motor[joint_motor]=0;
					line_follow=false;
					wall_follow=false;
				}
			}

			if( vexRT[ Btn6U ] && time1[T1] > 100 )
			{
				setpoint = setpoint + 100;
				ClearTimer(T1);
			}

			if( vexRT[ Btn6D ] && time1[T1] > 100 )
			{
				setpoint = setpoint - 100;
				ClearTimer(T1);
			}

			if( vexRT[ Btn5U ] )
			{
				motor[ claw_motor ] = 100;
			}
			else if( vexRT[ Btn5D ] )
			{
				motor[ claw_motor ] = -100;
			}
			else
			{
				motor[ claw_motor ] = 0;
			}
			//here down is the buttone controls
			if(vexRT[Btn8U])
			{
				setpoint = balloffset;
			}
			if(vexRT[Btn8D])
			{
				backward_distance(12);
			}

			if(vexRT[Btn8L])
			{
				//turn_left_90();
				line_follow=true;
				wall_follow=false;
			}

			if(vexRT[Btn8R])
			{
				//turn_right_90 ();
				line_follow=false;
				wall_follow=true;
			}
			if(vexRT[Btn7L])
			{
				scale=2;
			}
			if(vexRT[Btn7R])
			{
				scale=1;
			}
			//here above is the button controls


			if (line_follow)
			{
				//below is the line follower sensor stuff
				if(SensorValue[middle_line]>1500)
				{
					motor[right_motor]=63;
					motor[left_motor]=63;
				}
				else if(SensorValue[left_line]>1500)
				{
					motor[left_motor]=-63;
					motor[right_motor]=63;
					wait1Msec(10);
					motor[right_motor]=0;
					motor[left_motor]=0;
				}
				else  if(SensorValue[right_line]>1500)
				{
					motor[left_motor]=63;
					motor[right_motor]=-63;
					wait1Msec(10);
				}
					else
				{
				motor[left_motor]=63;
				motor[right_motor]=-63;
				wait1Msec(10);
				motor[left_motor]=63;
				motor[right_motor]=63;
				wait1Msec(10);
				}
			}
			//above is the line follower stuff

			//below is the limit switch stuff
			else if (wall_follow)
			{
				if(SensorValue[front_right_limit_switch] ==1 && SensorValue[back_right_limit_switch]== 1)
				{
					motor[left_motor]=63;
					motor[right_motor]=63;
				}
				if(SensorValue[front_right_limit_switch]==0)
				{
					motor[left_motor]=31;
					motor[right_motor]=-31;
					wait1Msec(250);
					motor[left_motor]=0;
					motor[right_motor]=0;
					wait1Msec(1000);
					forward_distance(10);
				}
				if(SensorValue[back_right_limit_switch]==0)
				{
					motor[right_motor]=31;
					motor[left_motor]=-31;
					wait1Msec(250);
					motor[left_motor]=0;
					motor[right_motor]=0;
					wait1Msec(1000);
					forward_distance(10);
				}
			}
		}
		//above is the limit switch stuff
	}
}


void forward_distance( float inches )
{
	int	milliseconds=(inches-5.96)/.0255;
	motor[left_motor]=127;
	motor[right_motor]=127;
	wait1Msec(milliseconds);
	motor[left_motor]=0;
	motor[right_motor]=0;
}

void backward_distance( float inches )
{
	int	milliseconds=(inches-5.96)/.0255;
	motor[left_motor]=-127;
	motor[right_motor]=-127;
	wait1Msec(milliseconds);
	motor[left_motor]=0;
	motor[right_motor]=0;
}

void turn_left_90()
{
	motor[right_motor]=45;
	motor[left_motor]=-45;
	wait1Msec(1665);
	motor[left_motor]=0;
	motor[right_motor]=0;
}

void turn_right_90 ()
{
	motor[left_motor]=45;
	motor[right_motor]=-45;
	wait1Msec(1665);
	motor[left_motor]=0;
	motor[right_motor]=0;
}

void arm_controller(int setpoint)
{
	motor[joint_motor]= .5 * (setpoint - SensorValue[pot_sensor]);
}
